//ANT+ code for Senior desing project 
//Team 4 at TAMU Fall2012-Spring2013
//
//Based off of code examples from thisisant.com


//********* Includes **************************
#include "./Includes/ANT_Utils.h"
#include "./Includes/ANT.h"
#include <xdc/runtime/Log.h>
//#include "/home/luke/Documents/Tamu/ECEN403/SeniorDesignCode/tamu-senior-design-fall12-spring13-team4/Includes/ANT_Utils.h"

//********* Forward Declaration ***************
void SetNetworkKey(char ucNetNum, char * ucNetKey);
void AssignChannel(char ucChanNum, char ucChanType, char ucNetNum);
void SetChannelID(char ucChanNum, char * usDeviceNum, char ucDeviceType, char ucTransType);
void SetChannelPeriod(char ucChanNum, char * usMessagePeriod);
void SetChannelRFFreq(char ucChanNum, char ucRF);
void setTXPower(char power);
void SetChannelTimeOut(char ucChanNum, char ucSearchTime);
void OpenChannel(char ucChanNum);
int ChannelResponse(char ucChanNum);


//************Main function ****************

//Assigns a channel to the network
void ResetAnt(){
  char message[5];

  message[0] = 0xA4;  //Sync bit
  message[1] = 1;     //Lenght of data
  message[2] = 0x4A;  //Assign Channel ID
  //Data payload
  message[3] = 0x00;

  message[4] = checksum(message,4); //Add checksum

  transmit_ANT(message,5);

  //delay for a 500ms
  volatile int i;
  for(i = 0; i < 10; i++)
    delay(45000);
}

// Sets up a RX Channel for a Heart Rate monitor
// ucTransType set to 0 if searching
// ucDeviceNum set to 0 if searching
int ConfigureRxChannelHRM( char ucTransType, char * usDeviceNum){
  volatile int i;
  char ucNetNum     = 0x00;     //use network num 0
  char ucChanNum    = HR_CHANNEL; //assign heart rate channel num
  char ucChanType   = 0x00;     //Receive or Slave Channel
  char ucDeviceType = 0x78;     //Set device type to 0x78
                                //which is a HRM
  char ucRF         = 0x39;     //Set the RF frequency to 
                                //channel 57 - 2.457GHz
  char ucSearchTime = 0x04;     //Set the search time to be 30
                                //seconds (set in 2.5s 
                                //increments) 4*2.5 = 10
  char ucPower = 0x03;

  // Set Network Key 
  char aucNetKey[8] = {0xB9,0xA5,0x21,0xFB,0xBD,0x72,0xC3,0x45};

  //Set the message period to 8070 counts specific for the HRM
  char usMessagePeriod[2] = {0x86,0x1F};

  // Calls to set up the channel and open it
  // repeating till a Response No Error is recieved (0x00)

  SetNetworkKey(ucNetNum, aucNetKey);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  AssignChannel(ucChanNum, ucChanType, ucNetNum);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  SetChannelID(ucChanNum, usDeviceNum, ucDeviceType, ucTransType);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  SetChannelRFFreq(ucChanNum, ucRF);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;
  
  SetChannelPeriod(ucChanNum, usMessagePeriod);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  setTXPower(ucPower);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  SetChannelTimeOut(ucChanNum, ucSearchTime);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  OpenChannel(ucChanNum);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  return 0;
}

reconectANT(char ucChanNum){
  OpenChannel(ucChanNum);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  return 0;
}

// Sets up a RX Channel for a Cadence and Speed Sensor
// ucTransType set to 0 if searching
// ucDeviceNum set to 0 if searching
int ConfigureRxChannelCAD( char ucTransType, char * usDeviceNum){
  volatile int i;
  char ucNetNum     = 0x00;     //use network num 0
  char ucChanNum    = CAD_CHANNEL; //assign cadence channel num
  char ucChanType   = 0x00;     //Receive or Slave Channel
  char ucDeviceType = 0x79;     //Set device type to 0x79
                                //which is a HRM
  char ucRF         = 0x39;     //Set the RF frequency to 
                                //channel 57 - 2.457GHz
  char ucSearchTime = 0x04;     //Set the search time to be 30
                                //seconds (set in 2.5s 
                                //increments) 4*2.5 = 10
  //char ucPower = 0x03;

  // Set Network Key 
  //char aucNetKey[8] = {0xB9,0xA5,0x21,0xFB,0xBD,0x72,0xC3,0x45};

  //Set the message period to 8070 counts specific for the HRM
  char usMessagePeriod[2] = {0x96,0x1F};

  // Calls to set up the channel and open it
  // repeating till a Response No Error is recieved (0x00)

  /*SetNetworkKey(ucNetNum, aucNetKey);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;*/

  AssignChannel(ucChanNum, ucChanType, ucNetNum);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  SetChannelID(ucChanNum, usDeviceNum, ucDeviceType, ucTransType);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  SetChannelRFFreq(ucChanNum, ucRF);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;
  
  SetChannelPeriod(ucChanNum, usMessagePeriod);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  /*setTXPower(ucPower);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;*/

  SetChannelTimeOut(ucChanNum, ucSearchTime);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  OpenChannel(ucChanNum);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  return 0;
}

// Sets up a RX Channel for a Heart Rate monitor
// ucTransType set to 0 if searching
// ucDeviceNum set to 0 if searching
int ConfigureRxChannelPROX( char ucTransType, char * usDeviceNum){
  volatile int i;
  char ucNetNum     = 0x00;     //use network num 0
  char ucChanNum    = PROX_CHANNEL; //assign cadence channel num
  char ucChanType   = 0x00;     //Receive or Slave Channel
  char ucDeviceType = 0x69;     //Set device type to 0x69
                                //which is a HRM
  char ucRF         = 0x39;     //Set the RF frequency to 
                                //channel 57 - 2.457GHz
  char ucSearchTime = 0x04;     //Set the search time to be 30
                                //seconds (set in 2.5s 
                                //increments) 4*2.5 = 10
  //char ucPower = 0x03;

  // Set Network Key 
  //char aucNetKey[8] = {0xB9,0xA5,0x21,0xFB,0xBD,0x72,0xC3,0x45};

  //Set the message period to 8070 counts specific for the HRM
  char usMessagePeriod[2] = {0x5F,0x3F};

  // Calls to set up the channel and open it
  // repeating till a Response No Error is recieved (0x00)

  /*SetNetworkKey(ucNetNum, aucNetKey);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;*/

  AssignChannel(ucChanNum, ucChanType, ucNetNum);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  SetChannelID(ucChanNum, usDeviceNum, ucDeviceType, ucTransType);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  SetChannelRFFreq(ucChanNum, ucRF);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;
  
  SetChannelPeriod(ucChanNum, usMessagePeriod);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  /*setTXPower(ucPower);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;*/

  SetChannelTimeOut(ucChanNum, ucSearchTime);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  OpenChannel(ucChanNum);
  //for(i = 0; i < 1000; i++);
  if(ChannelResponse(ucChanNum))
    return -1;

  return 0;
}

reconnectANT(char ucChanNum){
  OpenChannel(ucChanNum);
  if(ChannelResponse(ucChanNum))
    return -1;

  return 0;
}

//Sets up the network key
void SetNetworkKey(char ucNetNum, char * ucNetKey){
  int i;
  char message[13];

  message[0] = 0xA4;  //Sync bit
  message[1] = 9;     //Length of data
  message[2] = 0x46;  //Set Network Key ID
  message[3] = ucNetNum;  //Send the network number

  for(i = 0; i < 8; i++)
    message[4+i] = ucNetKey[i];
  
  message[12] = checksum(message,12); //Add checksum

  transmit_ANT(message,13);
}

//Assigns a channel to the network
void AssignChannel(char ucChanNum, char ucChanType, char ucNetNum){
  char message[7];
  
  message[0] = 0xA4;  //Sync bit
  message[1] = 3;     //Lenght of data
  message[2] = 0x42;  //Assign Channel ID
  //Data payload
  message[3] = ucChanNum;
  message[4] = ucChanType;
  message[5] = ucNetNum;
  
  message[6] = checksum(message,6); //Add checksum

  transmit_ANT(message,7);
}

//Assigns a channel ID to the network
void SetChannelID(char ucChanNum, char * usDeviceNum, char ucDeviceType, char ucTransType){
  char message[9];

  message[0] = 0xA4;  //Sync bit
  message[1] = 5;     //Lenght of data
  message[2] = 0x51;  //Assign Channel ID
  //Data payload
  message[3] = ucChanNum;
  message[4] = usDeviceNum[0];
  message[5] = usDeviceNum[1];
  message[6] = ucDeviceType;
  message[7] = ucTransType;
  
  message[8] = checksum(message,8); //Add checksum

  transmit_ANT(message,9);
}

//Assigns the period for searching
void SetChannelPeriod(char ucChanNum, char * usMessagePeriod){
  char message[7];

  message[0] = 0xA4;  //Sync bit
  message[1] = 3;     //Lenght of data
  message[2] = 0x43;  //Assign Channel ID
  //Data payload
  message[3] = ucChanNum;
  message[4] = usMessagePeriod[0];
  message[5] = usMessagePeriod[1];
  
  message[6] = checksum(message,6); //Add checksum

  transmit_ANT(message,7);
}

//Assigns the RF frequency for comunication
void SetChannelRFFreq(char ucChanNum, char usRF){
  char message[6];

  message[0] = 0xA4;  //Sync bit
  message[1] = 2;     //Lenght of data
  message[2] = 0x45;  //Assign Channel ID
  //Data payload
  message[3] = ucChanNum;
  message[4] = usRF;
  
  message[5] = checksum(message,5); //Add checksum

  transmit_ANT(message,6);
}

//Set Transmit power
void setTXPower(char power){
  char message[6];

  message[0] = 0xA4;  //Sync bit
  message[1] = 2;     //Lenght of data
  message[2] = 0x47;  //Assign Channel ID
  //Data payload
  message[3] = 0;
  message[4] = power;
  
  message[5] = checksum(message,5); //Add checksum

  transmit_ANT(message,6);
  
}

//Assigns the Time out for the Channel
void SetChannelTimeOut(char ucChanNum, char ucSearchTime){
  char message[6];

  message[0] = 0xA4;  //Sync bit
  message[1] = 2;     //Lenght of data
  message[2] = 0x44;  //Assign Time out
  //Data payload
  message[3] = ucChanNum;
  message[4] = ucSearchTime;

  message[5] = checksum(message,5); //Add checksum

  transmit_ANT(message,6);
}

//Opens channel
void OpenChannel(char ucChanNum){
  char message[5];

  message[0] = 0xA4;  //Sync bit
  message[1] = 1;     //Lenght of data
  message[2] = 0x4B;  //Assign Channel ID
  //Data payload
  message[3] = ucChanNum;
  
  message[4] = checksum(message,4); //Add checksum

  transmit_ANT(message,5);
  
}

//Returns the messge code from the Channel Response
//message 
//returns -1 if an error
int ChannelResponse(char ucChanNum){
  char message[7];
  int i;
  char resp;

  //Keep reading till see a message response then return
  //that value
  while(1){
    resp = read_ANT(message,3);
	if((!resp) && (message[2] == 0x40) && (message[3] == ucChanNum))
      break;
  }

  Log_info0("Message Value\n");
  for(i = 0; i < 7; i++)
    Log_info2("Message[%d] = 0x%X\n",i,message[i]);
  return(message[5]);
}
